#ifndef _LOCALIZER_H_
#define _LOCALIZER_H_


#include <Eigen/Core>
#include "pnp/pnp.h"

#include "indexer.h"

using Eigen::Vector2d;
using Eigen::Vector3d;

struct KnnFeat {
  int feat_id;
  int point_id;
  int cost;  // the cost is the number of 2d features 3d point have
  KnnFeat() {}
  KnnFeat(int _fid, int _i, int _c) :
    feat_id(_fid), point_id(_i), cost(_c) {}
  ~KnnFeat() {}
};

struct LocalizeResult {
  void clear() {
    cnt_okay = 0;
    time[0] = time[1] = time[2] = 0;
    err_pos = 0;
    err_angle = 0;
  }
  double cnt_okay;
  double time[3]; // matching, ransac, total
  double err_pos, err_angle; // mean, 1st quartile, median, 3rd quartile 
};

struct KnnComparor {
  bool operator() (const KnnFeat& n1, const KnnFeat& n2) {
    return n1.cost < n2.cost;
  }
};

class LocalizerHandler {
private:
  Indexer mIndexer;

  std::vector<cv::DMatch> mMatches_putative;

  const std::vector<Eigen::Vector3d>& mPoints3D;
  const std::vector<int>& mPoints3dCost;
  const std::vector<int>& mFeatureToPoints3d;

  // ratio test threshold to reject putative matches
  float mRatio_test_threshold;

  // threshold: the max number of matched feature and 3d point
  // when reach this threshold then stop to calculate camera pose
  int mMaxNumberCorrespondence;

  // threshold: the minimal number of matched correspondence
  // if there are not enough 2d-3d correspondence, localize fail.
  int mMinNumberCorrespondence;

  // find nearest neighbor for BRISK descriptor
  void FindKnn( const cv::Mat& desc, int desc_start_idx, std::vector<KnnFeat>& knn_feats );

public:
  LocalizerHandler(const std::vector<Eigen::Vector3d>& points3d,
                   const std::vector<int>& points_3d_cost,
                   const std::vector<int>& feature_to_points3d);
  ~LocalizerHandler();

  // load the db descriptors and index_file
  inline void InitIndexer(const cv::Mat& desc_mat,
                          const std::string& index_file,
                          const IndexMethod index_method);

  // opencv Point2f
  bool Locate(const pnp::PnPSolver& pnp,
              const std::vector<cv::Point2d>& key_pts,
              const cv::Mat& desc_mat,
              const int width, const int height,
              cv::Matx33d& r, cv::Vec3d& c,
              LocalizeResult& res);

  // self-defined KeyPoint
  bool Locate(const pnp::PnPSolver& pnp,
              const std::vector<KeyPoint>& key_pts,
              const std::vector<Descriptor>& desc_vec,
              const int width, const int height,
              cv::Matx33d& r, cv::Vec3d& c,
              LocalizeResult& res);

};

#endif
